/*
 * File: leg_pos.c
 *
 * MATLAB Coder version            : 5.5
 * C/C++ source code generated on  : 07-Jul-2024 15:58:41
 */

/* Include Files */

#include <math.h>

/*
 * LEG_POS
 *     POS = LEG_POS(PHI1,PHI4)
 *
 * Arguments    : float phi1
 *                float phi4
 *                float pos[2]
 * Return Type  : void
 */
void leg_pos(float phi1, float phi4, float pos[2])
{
  float a;
  float b_a;
  float t2;
  float t22;
  float t3;
  float t4;
  float t5;
  float t6;
  float t8_tmp;

  /*     This function was generated by the Symbolic Math Toolbox version 9.2.
   */
  /*     22-Jun-2024 16:49:15 */
  t2 = (float)cos(phi1);
  t3 = (float)cos(phi4);
  t4 = (float)sin(phi1);
  t5 = (float)sin(phi4);
  t6 = t2 / 20.0F;
  t8_tmp = t4 / 20.0F;
  a = t8_tmp - t5 / 20.0F;
  b_a = (t3 / 20.0F - t6) + 0.06F;
  t22 = t4 * 0.0105F - t5 * 0.0105F;
  t5 = (t3 * 0.0105F - t2 * 0.0105F) + 0.0126F;
  t4 = a * a + b_a * b_a;
  t4 = (float)atan(1.0F / (t5 + t4) *
                   (t22 + (float)sqrt((t22 * t22 + t5 * t5) - t4 * t4))) *
       2.0F;
  t5 = t8_tmp - (float)sin(t4) * 0.105F;
  t4 = (t6 + (float)cos(t4) * 0.105F) - 0.03F;
  pos[0] = (float)sqrt(t5 * t5 + t4 * t4);
  pos[1] = (float)atan2(t5, t4);
}

/*
 * File trailer for leg_pos.c
 *
 * [EOF]
 */
